#include "icm42688.h"
#include "iic1.h"

#define ICM42607_DEVICE_ADDR 0x68
#define WHO_AM_I 0x75
								
// 判断芯片是否存在
static uint8_t existenceFlag = 0;	
									
uint8_t icm42688Init(void) 			
{									
	uint8_t id = 0;
	i2c1Read(ICM42607_DEVICE_ADDR,WHO_AM_I,&id,1);

	if (id == 0x47)
	{
		existenceFlag = 1;
								
		uint8_t regData = 0x06;//默认值，输出1khz//	0x07;//输出200hz //0x09;输出50hz//0x06;默认值，输出1khz//	
		i2c1Write(ICM42607_DEVICE_ADDR, 0x4f, &regData, 1);//配置GURO输出频率和量程 2000dps 1khz
		i2c1Write(ICM42607_DEVICE_ADDR, 0x50, &regData, 1);//配置ACC输出频率和量程	16g 1khz
						
//		regData=0x16;	
//		i2c1Write(ICM42607_DEVICE_ADDR,0x51, &regData, 1);//gyro bandwidth 170
		regData=0x33; //0x22	//0x33~0x77//
		i2c1Write(ICM42607_DEVICE_ADDR,0x52, &regData, 1);//gyro ACC bandwidth 100
						
		regData = 0x0f;
		i2c1Write(ICM42607_DEVICE_ADDR, 0x4e, &regData, 1);//主要是开启陀螺仪，并配置功耗
	}
	return existenceFlag;
}

// 先用int16来计算校准值，再用float来算出最后结果
// 防止用float来校准丢失精度 
void getIcm42688Data(int16_t *acc, int16_t *gyro)
{
	if (existenceFlag)
	{	
		uint8_t data[12];
		i2c1Read(ICM42607_DEVICE_ADDR,0x1f, data, 12);
		int16_t t_sensorAcc[3];
		int16_t t_sensorGyro[3];
		t_sensorAcc[0] = (int16_t)(data[1] | (data[0] << 8));
		t_sensorAcc[1] = (int16_t)(data[3] | (data[2] << 8));
		t_sensorAcc[2] = (int16_t)(data[5] | (data[4] << 8));
		t_sensorGyro[0] = (int16_t)(data[7] | (data[6] << 8));
		t_sensorGyro[1] = (int16_t)(data[9] | (data[8] << 8));
		t_sensorGyro[2] = (int16_t)(data[11] | (data[10] << 8));
			
		acc[1] = t_sensorAcc[0];
		acc[0] = t_sensorAcc[1];
		acc[2] = t_sensorAcc[2];
		gyro[1] = -t_sensorGyro[0];
		gyro[0] = -t_sensorGyro[1];
		gyro[2] = t_sensorGyro[2];
	}
}


